package com.utils;

/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

/**
 *
 * @author Dave Brackeen, Keith Ballantyne
 */
import java.util.*;

/**
  The AStarSearch class, along with the AStarNode class,
  implements a generic A* search algorithm. The AStarNode
  class should be subclassed to provide searching capability.
*/
public class AStarSearch {


  /**
    A simple priority list, also called a priority queue.
    Objects in the list are ordered by their priority,
    determined by the object's Comparable interface.
    The highest priority item is first in the list.
  */
  public static class PriorityList extends LinkedList {

    @SuppressWarnings("unchecked")
    public void add(Comparable object) {
      for (int i=0; i<size(); i++) {
        if (object.compareTo(get(i)) <= 0) {
          add(i, object);
          return;
        }
      }
      addLast(object);
    }
  }


  /**
    Construct the path, not including the start node.
  */
  @SuppressWarnings("unchecked")
  protected List constructPath(AStarNode node) {
    LinkedList path = new LinkedList();

//    path.addFirst(node);
    
    while (node.pathParent != null) {
      path.addFirst(node);
//      System.err.print("addFirst("+node+") ");
      node = node.pathParent;
    } 
    
    return path;
  }


  /**
    Find the path from the start node to the end node. A list
    of AStarNodes is returned, or null if the path is not
    found. 
  */
  @SuppressWarnings("unchecked")
  public List findPath(AStarNode startNode, AStarNode goalNode) {

//        System.err.println("Finding path from " + startNode + " to " + goalNode);
        PriorityList openList = new PriorityList();
        LinkedList closedList = new LinkedList();

        startNode.costFromStart = 0;
        startNode.estimatedCostToGoal =
                startNode.getEstimatedCost(goalNode);
        startNode.pathParent = null;
        openList.add(startNode);

        while (!openList.isEmpty()) {
            AStarNode node = (AStarNode) openList.removeFirst();
            if (node.equals(goalNode)) {
                // construct the path from start to goal
//                System.err.println("Completed search, found solution!");
//                System.err.flush();
                return constructPath(node);
            }

            List neighbors = node.getNeighbors();
            for (int i = 0; i < neighbors.size(); i++) {
                AStarNode neighborNode =
                        (AStarNode) neighbors.get(i);
                boolean isOpen = openList.contains(neighborNode);
                boolean isClosed =
                        closedList.contains(neighborNode);
                float costFromStart = node.costFromStart
                        + node.getCost(neighborNode);

                // check if the neighbor node has not been
                // traversed or if a shorter path to this
                // neighbor node is found.
                if ((!isOpen && !isClosed)
                        || costFromStart < neighborNode.costFromStart) {
                    neighborNode.pathParent = node;
                    neighborNode.costFromStart = costFromStart;
                    neighborNode.estimatedCostToGoal =
                            neighborNode.getEstimatedCost(goalNode);
                    if (isClosed) {
                        closedList.remove(neighborNode);
//              System.err.println("Closing node "+neighborNode+" cost: "+neighborNode.costFromStart+" estTotal "+neighborNode.estimatedCostToGoal);
                    }
                    if (!isOpen) {
                        openList.add(neighborNode);
                        if (openList.size() > 50) {
//                            return null;
                            // should abort, but let's try a partial path
                            return constructPath(neighborNode);
                        }
//              System.err.println("Opening node "+neighborNode+" parent="+neighborNode.pathParent);
                    }
                }
            }
            closedList.add(node);
//              System.err.println("Closing node "+node+" cost: "+node.costFromStart+" estTotal "+node.estimatedCostToGoal);
        }

//        System.err.println("Completed search, NO PATH!");
//        System.err.flush();
        // no path found
        return null;
    }

}